
#include <ecal/ecal.h>

#include "argparse/argparse.hpp"
#include <chrono>
#include <iostream>
#include <mutex>
#include <thread>

#include "latency_log.h"

// warmup runs not to measure
const int warmups(100);

// data structure for later evaluation
struct SCallbackPar {
  SCallbackPar() { latency_array.reserve(100000); };
  std::mutex mtx;
  std::vector<long long> latency_array;
  size_t rec_size = 0;
  size_t msg_num = 0;
};

// message receive callback
void on_receive(const struct eCAL::SReceiveCallbackData* data_, SCallbackPar* par_, int delay_) {
  // get receive time stamp
  auto rec_time =
      std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
          .count();

  // update latency, size and msg number
  const std::lock_guard<std::mutex> lock(par_->mtx);
  par_->latency_array.push_back(rec_time - data_->time);
  par_->rec_size = data_->size;
  par_->msg_num++;
  // delay callback
  if (delay_ > 0) std::this_thread::sleep_for(std::chrono::milliseconds(delay_));
}

// single test run
void do_run(int delay_, std::string& log_file_) {
  // initialize eCAL API
  eCAL::Initialize(0, nullptr, "latency_rec");

  // subscriber
  eCAL::CSubscriber sub("ping");

  // apply subscriber callback function
  SCallbackPar cb_par;
  auto callback = std::bind(on_receive, std::placeholders::_2, &cb_par, delay_);
  sub.AddReceiveCallback(callback);

  size_t msg_last(0);
  while (eCAL::Ok()) {
    // check once a second if we still receive new messages
    // if not, we stop and evaluate this run
    {
      const std::lock_guard<std::mutex> lock(cb_par.mtx);
      if ((cb_par.msg_num > 0) && (msg_last == cb_par.msg_num))
        break;
      else
        msg_last = cb_par.msg_num;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  }

  // detach callback
  sub.RemReceiveCallback();

  // evaluate all
  evaluate(cb_par.latency_array, cb_par.rec_size, warmups, log_file_);

  // log all latencies into file
  log2file(cb_par.latency_array, cb_par.rec_size, log_file_);

  // finalize eCAL API
  eCAL::Finalize();
}

int main(int argc, char** argv) {
  try {
    // parse command line
    argparse::ArgumentParser program("latency_rec");
    program.add_argument("--delay").help("Callback process delay in ms.").default_value(0).scan<'i', int>();
    program.add_argument("--log_file")
        .help("Base file name to export results.")
        .default_value(std::string(""))
        .nargs(1);
    program.parse_args(argc, argv);
    int delay = program.get<int>("--delay");
    std::string log_file = program.get<std::string>("--log_file");

    // run tests
    while (eCAL::Ok()) {
      do_run(delay, log_file);
    }
  } catch (const std::exception& ex)  // catch any exceptions
  {
    std::cerr << "error: " << ex.what() << " for args." << std::endl;
    return -1;
  }

  return 0;
}
